#ifndef WORLD_SIMULATION_H
#define WORLD_SIMULATION_H

#include "Modeling/World.h"
#include "ODESimulator.h"
#include "ControlledSimulator.h"
#include <map>

struct ContactFeedbackInfo
{
  //summary information
  bool accum;      //set to true if you want to accumulate summary feedback over sub-steps
  bool hadContact,hadSeparation;
  Vector3 meanForce,meanPoint;

  //full contact information over sub-steps
  bool accumFull;  //set to true if all ODEContactLists should be stored
  vector<double> times;
  vector<ODEContactList> contactLists;
};

class WorldSimulation
{
public:
  WorldSimulation();
  void Init(RobotWorld* world);
  //sets the robot's controller -- note, this becomes the owner of c
  void SetController(int robot,RobotController* c);
  //advance simulation time by dt (may take multiple sub-steps)
  void Advance(Real dt);
  //takes the simulation state and puts it in the world model
  void UpdateModel(); 
  //load/save state
  bool ReadState(File& f);
  bool WriteState(File& f) const;
  bool ReadState(const string& data);
  bool WriteState(string& data) const;

  //contact querying routines
  //enables contact feedback between the two objects.  This is necessary
  //for most of the contact querying functions to work.
  //The exception is InContact, which works no matter what.
  void EnableContactFeedback(int aid,int bid,bool accum=true,bool accumFull=false);
  //returns true if the objects were in contact on the prior time step
  bool InContact(int aid,int bid=-1);
  //returns true if the objects had contact during past Advance call
  bool HadContact(int aid,int bid=-1);
  //returns true if the objects had no contact during past Advance call
  bool HadSeparation(int aid,int bid=-1);
  //returns the ContactFeedback structure for the two objects
  ContactFeedbackInfo* GetContactFeedback(int aid,int bid);
  //returns the contact list for the prior time step
  ODEContactList* GetContactList(int aid,int bid);
  //returns the resultant contact force (on object a) from the prior time step
  Vector3 ContactForce(int aid,int bid=-1);
  //returns the average contact force (on object a) from the prior Advance call
  Vector3 MeanContactForce(int aid,int bid=-1);

  //helpers to convert indexing schemes
  int ODEToWorldID(const ODEObjectID& odeid) const;
  ODEObjectID WorldToODEID(int id) const;

  RobotWorld* world;
  ODESimulator odesim;
  Real time;
  Real simStep;
  vector<ControlledRobotSimulator> controlSimulators;
  vector<SmartPointer<RobotController> > robotControllers;
  typedef map<pair<ODEObjectID,ODEObjectID>,ContactFeedbackInfo> ContactFeedbackMap;
  ContactFeedbackMap contactFeedback;
};

#endif
